#include <Arduino.h>
// #define USE_TEENSY_HW_SERIAL 1


#undef ESP32
#include <ros.h>
#define ESP32
// #include <ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;
// ros::Publisher chatter("chatter", &str_msg);
ros::Publisher cmd_publisher("esp32_ctrl", &str_msg);


char rosserial_json[80] = "{\"x\":1,\"y\":0,\"z\":0,\"th\":0,\"speed\":1,\"turn\":1,\"yawAjust\",0.12}";
		// 'i':(1,0,0,0),
const char rosserialCmdFormat[] =  "{\"x\":%1d,\"y\":%1d,\"z\":%1d,\"th\":%1d,\"speed\":%2.1f,\"turn\":%2.1f,\"yawAjust\":%2.1f}";
char rosserial_cmd_typ = 'S' ;         //命令控制类型
float rosserial_cmd_val = 0.0 ;        //命令控制PID输出

//----------------------
#include <ESP32Servo.h>


#include <EEPROM.h>
#include <WiFi.h>
#include <ESPmDNS.h>

#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <AsyncElegantOTA.h>

#define MQTT_MAX_PACKET_SIZE 512              //override MQTT_MAX_PACKET_SIZE(256) in pubsubclient.h, to fit the mqtt payload needs
#include <PubSubClient.h>                     //这条include 必须在 MQTT_MAX_PACKET_SIZE 定义之后，否则会有警告
#include <ArduinoJson.h>


//----------------------------------------
#define MONITOR_BAUD  115200
#define TX_PIN 21         // Rx pin which the S8 Tx pin is attached to (change if it is needed)
#define RX_PIN 19         // Tx pin which the S8 Rx pin is attached to (change if it is needed)

#define DEBUG_SERIAL (Serial1)          //用于打印LOG日志的串口
//---------EEPROM 地址分区映射----------
#define EEPROM_SIZE   512               //存储和恢复数据点，防止重启后执行器状态变更
#define WIFI_CON_MAX_TRY  50            //WIFI最大尝试连接次数
#define MQTT_CON_MAX_TRY  5             //MQTT最大尝试连接次数
#define FW_VERSION   1.2                    //程序版本号，用于升级标识
#define MQTT_UPLOAD_CYCLE 30                //mqtt 主动上报周期:秒

typedef enum{FSM_1 = 1, FSM_2=2} x_rob_stat_t;


//-----------控制参数--------------
char rob_wk_mod = 'S';                        //机器人运行模式，将来被状态机替代
// #define KP 1.0
// #define KI 0.5
// #define KD 0.3
float KP = 1.0;
float KI = 0.5;
float KD = 0.3;
#define I_MAX_VAL 90                         //积分限定上限
#define I_MIN_VAL -90                         //积分限定

const float def_speed = 0.05;                 //默认线速度
const float def_turn =  0.05;                 //默认旋转速度

//----------------------------------------
typedef struct dp_tag
{
  char cmd[10];           //控制指令
  int32_t x;              //
  int32_t y;              //
  int32_t z;              //


  float kp,ki,kd;         //pid参数
  float trail_error;      //寻迹误差        bit1 bit0 | bit0 bit1 以中心轴为0点，二进制权重依次增加表示偏移量

  x_rob_stat_t robStat;   //状态机变量

  uint16_t rssi;          //RSSI
} dev_data_t;


// Servo pickServo; 				  // create servo object to control a servo

dev_data_t x_devdatapoint; //数据点

// Replace with your network credentials
const char* ssid     = "xjaujsj";
const char* password = "1223334444";


// String device_rec_topic = JXB_CtrlerNO1_ID + "/+" ;
const char* userName = "xjau";                            //这里用户名和密码可以是企业用户，也可以是web用户
const char* passWord = "xjau1952";                        //尽量使用企业用户，然后用二维码绑定到web用户
// Add your MQTT Broker IP address
const char* mqtt_server = "192.168.1.151";
const uint16_t mqtt_port = 1883;

String deviceID = "esp32_car_ctrlr";                               //xyz+剪刀控制器ID

String trailingCtrlerID = deviceID + "_upload";           //寻迹设备MQTT topic

WiFiClient espClient;
PubSubClient client(espClient);
String uploadJson;                                       // mqtt publish payload

AsyncWebServer server(80);


//-------------------寻迹传感器引脚定义-----------------

const int IR_SENS_F1 = 36;       //定义小车前端第一个引脚为F1 front
const int IR_SENS_F2 = 39;       //定义小车前端第一个引脚为F2
const int IR_SENS_F3 = 34;       //定义小车前端第一个引脚为F3
const int IR_SENS_F4 = 35;       //定义小车前端第一个引脚为F4

const int IR_SENS_B1 =  4;       //定义小车前端第一个引脚为F1 front
const int IR_SENS_B2 = 17;       //定义小车前端第一个引脚为F2
const int IR_SENS_B3 =  5;       //定义小车前端第一个引脚为F3
const int IR_SENS_B4 = 18;       //定义小车前端第一个引脚为F4


const int IR_SENS_L1 = 33;        //定义小车左侧第一个引脚为L1 left
const int IR_SENS_L2 = 25;       //定义小车左侧第一个引脚为L2
const int IR_SENS_L3 = 26;        //定义小车左侧第一个引脚为L3
const int IR_SENS_L4 = 27;       //定义小车左侧第一个引脚为L4

const int IR_SENS_R1 = 13;       //定义小车右侧第一个引脚为R1 right
const int IR_SENS_R2 = 15;       //定义小车右侧第一个引脚为R2
const int IR_SENS_R3 =  2;       //定义小车右侧第一个引脚为R3
const int IR_SENS_R4 = 12;       //定义小车右侧第一个引脚为R4

//-------------------寻迹传感器-----------------


//------------IPC线程通信对象-----------
typedef struct msg_q_tag {
  char cmd[10];          //设备控制命令
  int32_t v1;            //控制命令参数1
  int32_t v2;            //控制命令参数2
  int32_t v3;            //控制命令参数3
} ctrl_msg_queue_t;     //接收到的MOSQUITTO命令解析后发往执行线程的MSG Q

xQueueHandle g_mqtt_ctrl_msgQ;      //tlink下发控制命令消息队列句柄

//------TASK HANDLER-------
TaskHandle_t mqttTaskHandle = NULL;                  // mqtt keyservice tsk handle
TaskHandle_t mqttExcutorTaskHandle = NULL;           // mqtt cmd executor tsk handle
TaskHandle_t rosserialTaskHandle = NULL;             // rosserial tsk handle
TaskHandle_t taillingTaskHandle = NULL;              // tailling line tsk handle

uint8_t dataPoint_EPPROM_shadowReg[EEPROM_SIZE];     //创建要放入EEPROM的数组，用于memecp 到 x_devdatapoint数据点


//-------------------------------------------------------------------
//++++++++++++++++++++++++++++ static function define ++++++++++++++++++++++++++++++++++
//----函数声明-----
void system_software_reset(void);
void mqtt_upload_json(void);


/********************************************************
* Function name: setup_wifi
* Description  : poll for modbus holding reg data
* Parameter    : void

* return        void
**********************************************************/
static void setup_wifi(void) {
  uint8_t wifi_connect_counter = WIFI_CON_MAX_TRY;

  WiFi.disconnect();               //防止已经连接了WIFI,显著提升复位后WIFI 连接速度
  delay(10);
  // start by connecting to a WiFi network
  DEBUG_SERIAL.printf("\r\nConnecting to %s\r\n", ssid);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);      //connect to wifi AP

  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    DEBUG_SERIAL.print(".");
    if(--wifi_connect_counter < 1){
      system_software_reset();        //save datapoint and reset
    }
  }

  DEBUG_SERIAL.println("WiFi connected!");
  DEBUG_SERIAL.println("IP address: ");
  DEBUG_SERIAL.println(WiFi.localIP());
}


/********************************************************
* Function name: OTA_webserver_init
* Description  : AsyncElegantOTA 升级初始化，创建两个路由：
                 /根目录展示json
                 /update 升级fermware
* Parameter    : void

* return        void
**********************************************************/
void OTA_webserver_init(void){
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
    request->send(200, "text/plain", uploadJson.c_str());
  });

  AsyncElegantOTA.begin(&server);    // Start ElegantOTA
  server.begin();
  DEBUG_SERIAL.println("HTTP server started, OTA server started");
}


/********************************************************
* Function name: callback
* Description  : MQTT 接收消息回调函数
* Parameter    :
* @topic:      传感器节点收到的 mqtt topic
* @message:    传感器节点收到的 mqtt message payload
* @length:     payload长度
* return        void
**********************************************************/
void mqtt_rec_callback(char* topic, byte* message, unsigned int length) {

  ctrl_msg_queue_t mqtt_ctrl_msg;                       //接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量
  String mqttTopic(topic);
  String messageJson(message,length) ;                   //mqtt message from server,字符数组构造字符串对象
  DEBUG_SERIAL.printf("\r\nmqtt topic: %s\r\n",mqttTopic.c_str());     //print message
  DEBUG_SERIAL.printf("\r\nMessage payload: %s\r\n",messageJson.c_str());     //print message

  // String input;

  StaticJsonDocument<256> doc;

  DeserializationError error = deserializeJson(doc, messageJson);

  if (error) {
    DEBUG_SERIAL.print("deserializeJson() failed: ");
    DEBUG_SERIAL.println(error.c_str());
    return;
  }

  // long x = doc["x"]; // -1
  // long y = doc["y"]; // -1
  // long z = doc["z"]; // -1
  long v1 = doc["v1"]; // -1
  long v2 = doc["v2"]; // -1
  long v3 = doc["v3"]; // -1
  // long rssi = doc["rssi"]; // 65535
  const char* cmd = doc["cmd"]; // "AMOV"
  // int pwm1 = doc["pwm1"]; // 1234
  // int pwm2 = doc["pwm2"]; // 4567
  // int delta_x = doc["delta_x"]; // 1
  // int delta_y = doc["delta_y"]; // 1
  // int delta_z = doc["delta_z"]; // 1

  memcpy(mqtt_ctrl_msg.cmd, cmd, sizeof("CMDT"));     //命令长度固定为4byte
  mqtt_ctrl_msg.v1 = v1;
  mqtt_ctrl_msg.v2 = v2;
  mqtt_ctrl_msg.v3 = v3;

	if (xQueueSend(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg, 2000) == pdPASS)
  {
    DEBUG_SERIAL.println("mqtt cmd send to execute thread");
  }

  // mqtt_upload_json();     //通过数据下发功能，可触发callback的 ACK，达到按照用户需求实时刷新目的

}
/********************************************************
* Function name: mqtt_reconnect
* Description  : 重新连接MOSQUITTO 服务器，超过最大连接数后重启ESP32
* Parameter    : void

* return        void
**********************************************************/
void mqtt_reconnect(void) {
  uint8_t recon_counter = MQTT_CON_MAX_TRY;   //最大重连5次

    if (client.connect(deviceID.c_str(),userName,passWord)) {
      DEBUG_SERIAL.println("device connected");
      // Subscribe
      String device_rec_topic = "/" + trailingCtrlerID + "/#" ;   //订阅设备所有topic /XYZ滑台TOPIC
      client.subscribe(device_rec_topic.c_str());                  //订阅设备所有topic
      DEBUG_SERIAL.printf("esp32 subscribe topic: %s\r\n",device_rec_topic.c_str());
    } else {
      DEBUG_SERIAL.print("failed, rc=");DEBUG_SERIAL.print("failed, rc=");
      DEBUG_SERIAL.print(client.state());
      DEBUG_SERIAL.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  // Loop until we're reconnected
  while (!client.connected()) {
    if(--recon_counter < 1){
      DEBUG_SERIAL.println("reconnect failed after 5 times of trying, reset ESP32...");
      system_software_reset();        //save datapoint and reset
    }

    // DEBUG_SERIAL.print("Attempting MQTT connection...");
    // Attempt to connect
    // if (client.connect(deviceID.c_str(),userName,passWord)) {
    //   DEBUG_SERIAL.println("device connected");
    //   // Subscribe
    //   String device_rec_topic = "/" + trailingCtrlerID + "/#" ;   //订阅设备所有topic /XYZ滑台TOPIC
    //   client.subscribe(device_rec_topic.c_str());                  //订阅设备所有topic
    //   DEBUG_SERIAL.printf("esp32 subscribe topic: %s\r\n",device_rec_topic.c_str());
    // } else {
    //   DEBUG_SERIAL.print("failed, rc=");DEBUG_SERIAL.print("failed, rc=");
    //   DEBUG_SERIAL.print(client.state());
    //   DEBUG_SERIAL.println(" try again in 5 seconds");
    //   // Wait 5 seconds before retrying
    //   delay(5000);
    // }
  }
}


/********************************************************
* Function name: mqtt_connect
* Description  : connect to mqtt broker
* Parameter    : void

* return        void
**********************************************************/
static void mqtt_connect(void) {
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(mqtt_rec_callback);
  // mqtt_reconnect();                     //连接tlink server
}

/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
static void datapoint_to_json(void){
    //-------通过修改topic的末尾数据，可实现连续多个设备的数据转发上报功能，是数据网关的关键功能！

    //---------UPDATE RELAY STATE----------
    uploadJson.clear();
    uploadJson =  "{\"cmd_t\":";
    uploadJson += rosserial_cmd_typ;
    uploadJson +=  ",\"cmd_v\":";
    uploadJson += rosserial_cmd_val;
    uploadJson +=  ",\"error\":";
    uploadJson += x_devdatapoint.z;
    uploadJson +=  ",\"rssi\":";
    uploadJson += x_devdatapoint.rssi;
    // uploadJson += "{\"flag\":\"a_t\",\"value\":";

    // uploadJson += x_devdatapoint.rssi;
    // // uploadJson += "},{\"flag\":\"FW\",\"value\":";
    // // uploadJson += FW_VERSION;
    uploadJson += "}";

    DEBUG_SERIAL.println(uploadJson.c_str());

}


/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
void mqtt_upload_json(void){
    static uint8_t mqttUploadCnt = 10;
    datapoint_to_json();  // datapoint 对象，拼接成json，结果存在uploadJson全局字符串中

    // bool pubRes = client.publish(JXB_CtrlerNO1_ID.c_str(),uploadJson.c_str());      //发布json数据
    bool pubRes = client.publish(("/"+deviceID).c_str(),uploadJson.c_str());      //发布json数据
    DEBUG_SERIAL.printf("esp32 pub topic:%s,payload:%s\r\n",deviceID.c_str(),uploadJson.c_str());
    while(mqttUploadCnt > 1)
    {
      mqttUploadCnt --;
      // system_software_reset();
    }
      if(pubRes != false){
         mqttUploadCnt = 10;
      }
}



/********************************************************
* Function name: tsk_mqt_mqtt_keepalive
* Description  : 连接并保活MOSQUITTO MQTT服务器连接
* Parameter    : pvParam 线程创建传入参数


* return         void
**********************************************************/
void tsk_mqtt_service(void *pvParam)
{
  mqtt_connect();                                        //连接MOSQUITTO

  delay(1000);                                                  //
  // mqtt_upload_json();                                           //upload json mqtt data 第一次设备上线上报数据
  while (1)
  {
   mqtt_reconnect();                     //连接tlink server
   client.loop();                              //MQTT消息处理函数
   delay(100);
  }
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++

/********************************************************
* Function name:  rosserial_node_init
* Description  : 初始化rosserial node, 创建发布器，发布/esp32_ctrl 的rostopic
* Parameter    : pvParam void

* return         void
**********************************************************/
void rosserial_node_init(void){
  DEBUG_SERIAL.begin(MONITOR_BAUD, SERIAL_8N1, RX_PIN, TX_PIN );
  // nh.initNode();
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  // nh.advertise(chatter);
  nh.advertise(cmd_publisher);
  DEBUG_SERIAL.println("rosserial init ok");

}
/********************************************************
* Function name:  format_rosserial_cmd
* Description  :  打包rosserial命令包，
* Parameter    : dir F: 前进
                     B: 后退
                     L: 左平移
                     R: 右平移
KP*front_sensor_val
* return         void
**********************************************************/
void format_rosserial_cmd(char dir, float pid_ajust)
{
  // pid_ajust = -pid_ajust;                           //逻辑反了
  memset(rosserial_json,0,sizeof(rosserial_json));
  switch (dir)
  {
  case 'S':
    sprintf(rosserial_json, rosserialCmdFormat, 0,0,0,0,0,0,0);
    break;
  case 'F':
    sprintf(rosserial_json, rosserialCmdFormat, 1,0,0,0,def_speed,def_turn,pid_ajust);
    break;
  case 'B':
    sprintf(rosserial_json, rosserialCmdFormat, -1,0,0,0,def_speed,def_turn,pid_ajust);
    break;
  case 'L':
    sprintf(rosserial_json, rosserialCmdFormat, 0,0,0,1,def_speed,def_turn,pid_ajust);
    break;
  case 'R':
    sprintf(rosserial_json, rosserialCmdFormat, 0,0,0,-1,def_speed,def_turn,pid_ajust);
    break;
  //------------向左自旋转-------------
  case '<':
    sprintf(rosserial_json, rosserialCmdFormat, 0,0,1,0,def_speed,def_turn,pid_ajust);      //舵轮角度固定，z*def_speed作为实心轮的速度
    break;
  //------------向右自旋转-------------
  case '>':
    sprintf(rosserial_json, rosserialCmdFormat, 0,0,-1,0,def_speed,def_turn,pid_ajust);     //舵轮角度固定，z*def_speed作为实心轮的速度
    break;

  default:
    break;
  }
  DEBUG_SERIAL.printf("rosserial msg:%s\r\n",rosserial_json);

}

/********************************************************
* Function name:
* Description  : 连接并保活MOSQUITTO MQTT服务器连接
* Parameter    : pvParam 线程创建传入参数


* return         void
**********************************************************/
void tsk_rosserial_publish(void *pvParam)
{
  rosserial_node_init();                                        //初始化rosserial node

  delay(100);                                                  //
  while (1)
  {
    // format_rosserial_cmd('F',front_yaw_pidout);           //控制命令通过rosserial发送
    // format_rosserial_cmd('B', back_yaw_pidout);               //控制命令通过rosserial发送
    // format_rosserial_cmd('L', left_yaw_pidout);               //控制命令通过rosserial发送
    // format_rosserial_cmd('R', right_yaw_pidout);               //控制命令通过rosserial发送
    // format_rosserial_cmd('<', 0);               //控制命令通过rosserial发送
    // format_rosserial_cmd('>', 0);               //控制命令通过rosserial发送
    format_rosserial_cmd(rosserial_cmd_typ, rosserial_cmd_val);               //控制命令通过rosserial发送
    str_msg.data = rosserial_json;
    cmd_publisher.publish(&str_msg);
    nh.spinOnce();
    delay(10);
  }
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++



/********************************************************
* Function name:  tailling_gpio_init
* Description  :  初始化寻迹模组的GPIO,低电平指示遇到黑线
* Parameter    :  pvParam void

* return         void
**********************************************************/
void tailling_gpio_init(void){
  //-------front 前方寻迹模组IO初始化---------
  pinMode(IR_SENS_F1, INPUT_PULLUP);
  pinMode(IR_SENS_F2, INPUT_PULLUP);
  pinMode(IR_SENS_F3, INPUT_PULLUP);
  pinMode(IR_SENS_F4, INPUT_PULLUP);
  //-------front 左方寻迹模组IO初始化---------
  pinMode(IR_SENS_L1, INPUT_PULLUP);
  pinMode(IR_SENS_L2, INPUT_PULLUP);
  pinMode(IR_SENS_L3, INPUT_PULLUP);
  pinMode(IR_SENS_L4, INPUT_PULLUP);
  //-------front 右方寻迹模组IO初始化---------
  pinMode(IR_SENS_R1, INPUT_PULLUP);
  pinMode(IR_SENS_R2, INPUT_PULLUP);
  pinMode(IR_SENS_R3, INPUT_PULLUP);
  pinMode(IR_SENS_R4, INPUT_PULLUP);
  //-------front 右方寻迹模组IO初始化---------
  pinMode(IR_SENS_B1, INPUT_PULLUP);
  pinMode(IR_SENS_B2, INPUT_PULLUP);
  pinMode(IR_SENS_B3, INPUT_PULLUP);
  pinMode(IR_SENS_B4, INPUT_PULLUP);


  DEBUG_SERIAL.println("tailling_gpio init ok");

}
/********************************************************
* Function name: tsk_tailling_for_strawberry
* Description  : 通过3组光电传感器寻迹整个场地，并接收机械臂的停机指令，停机信号撤销后继续寻迹到终点
* Parameter    : pvParam 线程创建传入参数


* return         void
**********************************************************/
void tsk_tailling_for_strawberry(void *pvParam)
{

  int8_t taillingSensError = 0;         //此变量为xxx_sensor_val的影子寄存器

  //--------借助二进制权重，  排列方式： bit-2 bit-1 bit1 bit2 = sens_n2 sens_n1 sens_p1 sens_p2 -------
  int8_t sens_n2 = 0;                   //红外反射光电传感器1，位权重-(2的2次方)
  int8_t sens_n1 = 0;                   //红外反射光电传感器2，位权重-(2的1次方)
  int8_t sens_p1 = 0;                   //红外反射光电传感器3，位权重(2的1次方)
  int8_t sens_p2 = 0;                   //红外反射光电传感器4，位权重(2的2次方)

  int8_t front_sensor_val = 0;          //0为正中， 负数偏左，正数偏右
  int8_t back_sensor_val = 0;           //0为正中， 负数偏左，正数偏右
  int8_t left_sensor_val = 0;           //0为正中， 负数偏左，正数偏右
  int8_t right_sensor_val = 0;          //0为正中， 负数偏左，正数偏右

  #define SENS2ERRRO 1                  //传感器值与误差的比例

  int16_t turnning_speed = 0;            //源地旋转角度，单位：度


  unsigned long preSensTime = 0;        // will store last time LED was updated
  // constants won't change :
  const long interval = 100;           // interval at which to blink (milliseconds)

  float previousError = 0;
  float integral = 0;
  float error = 0;
  float pid_out = 0;



  tailling_gpio_init();                 //初始化rosserial node


  while (1)
  {
      //---前方四个传感器----
      sens_n2 = digitalRead(IR_SENS_F1) == 0 ? 1 : 0;
      sens_n1 = digitalRead(IR_SENS_F2) == 0 ? 1 : 0;
      sens_p1 = digitalRead(IR_SENS_F3) == 0 ? 1 : 0;
      sens_p2 = digitalRead(IR_SENS_F4) == 0 ? 1 : 0;
      taillingSensError = sens_n2 * (-4) + sens_n1 * (-2) +  sens_p1 * 2 +  sens_p2 * 4;
      front_sensor_val = taillingSensError;        //线序插反
      //---后方四个传感器----
      sens_n2 = digitalRead(IR_SENS_B1) == 0 ? 1 : 0;
      sens_n1 = digitalRead(IR_SENS_B2) == 0 ? 1 : 0;
      sens_p1 = digitalRead(IR_SENS_B3) == 0 ? 1 : 0;
      sens_p2 = digitalRead(IR_SENS_B4) == 0 ? 1 : 0;
      taillingSensError = sens_n2 * (-4) + sens_n1 * (-2) +  sens_p1 * 2 +  sens_p2 * 4;
      back_sensor_val = taillingSensError;          //线序插反
      //---左方四个传感器----
      sens_n2 = digitalRead(IR_SENS_L1) == 0 ? 1 : 0;
      sens_n1 = digitalRead(IR_SENS_L2) == 0 ? 1 : 0;
      sens_p1 = digitalRead(IR_SENS_L3) == 0 ? 1 : 0;
      sens_p2 = digitalRead(IR_SENS_L4) == 0 ? 1 : 0;
      taillingSensError = sens_n2 * (-4) + sens_n1 * (-2) +  sens_p1 * 2 +  sens_p2 * 4;
      left_sensor_val = taillingSensError;           //线序插反
      //---右方四个传感器----
      sens_n2 = digitalRead(IR_SENS_R1) == 0 ? 1 : 0;
      sens_n1 = digitalRead(IR_SENS_R2) == 0 ? 1 : 0;
      sens_p1 = digitalRead(IR_SENS_R3) == 0 ? 1 : 0;
      sens_p2 = digitalRead(IR_SENS_R4) == 0 ? 1 : 0;
      taillingSensError = sens_n2 * (-4) + sens_n1 * (-2) +  sens_p1 * 2 +  sens_p2 * 4;
      right_sensor_val = taillingSensError;

      // DEBUG_SERIAL.printf("FRONT ERR:%d\r\nBACK ERR:%d\r\nLEFT ERR:%d\r\nRIGHT ERR:%d\r\n",front_sensor_val,back_sensor_val,left_sensor_val,left_sensor_val);

      //-------------------------PID------------------------------

      switch (rob_wk_mod){
        case 'F':
          error = SENS2ERRRO*front_sensor_val;
        break;
        case 'B':
          error = SENS2ERRRO*back_sensor_val;
        break;
        case 'L':
          error = SENS2ERRRO*left_sensor_val;
        break;
        case 'R':
          error = SENS2ERRRO*right_sensor_val;
        break;
        case '<':
          error = SENS2ERRRO;
        break;
        case '>':
          error = -SENS2ERRRO;
        break;
        case 'S':
        default: {error = 0;
                  rob_wk_mod = 'S';}      //其他，停止

      }
      //-----pid计算------

      integral += error;
      if(integral > I_MAX_VAL) integral = I_MAX_VAL;
      if(integral < I_MIN_VAL) integral = I_MIN_VAL;
      // float derivative = error - previousError;
      // float pid_out = KP * error + KI * integral + KD * derivative;
      pid_out = KP * error + KI * integral;
      previousError = error;
      // adjustMotors(pid_out);
      rosserial_cmd_typ = rob_wk_mod;       //可以用消息邮箱更新rosserial publish线程中的命令类型和调控值
      rosserial_cmd_val = pid_out;

      delay(10); // 延迟10毫秒
  }
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++

/********************************************************
* Function name: tsk_executer_ctrl
* Description  : mqtt 命令接收解析处理 线程函数
* Parameter    : pvParam 线程创建传入参数

                 命令说明：rssi=-1 进入OTA远程升级
                          rssi=0 远程重启
                          rssi为其他值，招测命令

* return         void
**********************************************************/

void tsk_executer_ctrl(void *pvParam)
{
  ctrl_msg_queue_t mqtt_ctrl_msg_rec;         //接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量
  long KPx1000 = 1.0;      //x1000用证书传值，精确到小数点3位
  long KIx1000 = 0.5;
  long KDx1000 = 0.3;

  while (1)
  {
    if (xQueueReceive(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg_rec, MQTT_UPLOAD_CYCLE*1000) == pdTRUE){
      DEBUG_SERIAL.println("MQTT--->ESP32: ctrl massage received!!!");
		  if( String(mqtt_ctrl_msg_rec.cmd) == "OTAU"){           // OTA 升级 OTAUPDATE
          DEBUG_SERIAL.println("ESP32 ENTER OTA MODE >>>>>>>>>>>");
          vTaskDelete(mqttTaskHandle);                        //删除mqtt任务，解决OTA与mqtt任务冲突问题
          vTaskDelete(rosserialTaskHandle);                   //删除mqtt任务，解决OTA与mqtt任务冲突问题
          vTaskDelete(taillingTaskHandle);                    //删除mqtt任务，解决OTA与mqtt任务冲突问题

          if(MDNS.begin(deviceID.c_str()))                    //设置DNS域名  http://$JXB_CtrlerNO1_ID.local, 通过MOSQUITTO sn,区分域名
            {
              DEBUG_SERIAL.println("MDNS started");
              DEBUG_SERIAL.println(deviceID +".local");
            }
          DEBUG_SERIAL.printf("OTA URL: %s\r\n",(deviceID+".local"+"/update").c_str()); //OTA升级URL

          datapoint_to_json();                              // 生成json字符串，供webserver显示，输出uploadJson全局变量
          OTA_webserver_init();                             // OTA webserver 任务初始化，http://192.168.119.233/update访问OTA更新页面
          // delay(mqtt_ctrl_msg_rec.v1*1000);                 // OTA升级后，重启，一般不会运行到此处，OTA更新完毕自动重启
          delay(60*1000);                                // OTA升级后，重启，一般不会运行到此处，OTA更新完毕自动重启
          DEBUG_SERIAL.println("OTA TIMEOUT,ESP32 IS RESETING...");
          ESP.restart();                                    // 重启ESP32
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "MOVF"){      // MOVE FORWARD
          rob_wk_mod = 'F';               //前进模式
          KPx1000 = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          KIx1000 = mqtt_ctrl_msg_rec.v2;
          KDx1000 = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "MOVB"){      // MOVE BACKWARD
          rob_wk_mod = 'B';               //后退模式
          KPx1000 = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          KIx1000 = mqtt_ctrl_msg_rec.v2;
          KDx1000 = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "MOVL"){      // MOVE LEFT
          rob_wk_mod = 'L';               //左转模式
          KPx1000 = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          KIx1000 = mqtt_ctrl_msg_rec.v2;
          KDx1000 = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "MOVR"){      // MOVE RIGHT
          rob_wk_mod = 'R';               //右转模式
          KPx1000 = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          KIx1000 = mqtt_ctrl_msg_rec.v2;
          KDx1000 = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "RO2L"){      // ROTATE TO LEFT
          rob_wk_mod = '<';               //右转模式
          KPx1000 = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          KIx1000 = mqtt_ctrl_msg_rec.v2;
          KDx1000 = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "RO2R"){      // ROTAE TO RIGHT
          rob_wk_mod = '>';               //停止模式
          // KP = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          // KI = mqtt_ctrl_msg_rec.v2;
          // KD = mqtt_ctrl_msg_rec.v3;
      }
      else if( String(mqtt_ctrl_msg_rec.cmd) == "STOP"){      // ROTAE TO RIGHT
          rob_wk_mod = 'S';               //停止模式
          // KP = mqtt_ctrl_msg_rec.v1;      //调整PID参数
          // KI = mqtt_ctrl_msg_rec.v2;
          // KD = mqtt_ctrl_msg_rec.v3;
      }

      //----------PID控制参数调整---------
      KP = KPx1000 * 0.001;      //KPx1000转换为真正参与运算的KP
      KI = KIx1000 * 0.001;
      KD = KDx1000 * 0.001;
    }//endofif (xQueueReceive(g_mqtt_ctrl_msgQ,
     //-----------消息队列超时控制自主上报周期，消息队列收到立即上报-------
    // mqtt_upload_json();         //upload json mqtt data
  } //end of while(1)
}





/********************************************************
* Function name: NVS_load_dataPoint
* Description  : 从non-volatile storage（flash）中载入数据点，恢复复位前状态
* Parameter    :

* return         void
**********************************************************/

static void NVS_load_dataPoint(void){
   EEPROM.begin(EEPROM_SIZE);            // initialize EEPROM with predefined size

   for(uint8_t i=0;i<sizeof(dev_data_t);i++){
    dataPoint_EPPROM_shadowReg[i] = EEPROM.read(i);
  }
  //--------RESTOR data point----------
  memcpy((void*)(&x_devdatapoint),dataPoint_EPPROM_shadowReg,sizeof(dev_data_t)); //数据恢复到g_dev_data中
  DEBUG_SERIAL.println("EEPROM data restore finished");

  //--------RESTOR EXCUTOR STATE----------

}

/********************************************************
* Function name: NVS_save_dataPoint
* Description  : 将设备数据点存入EEPROM
* Parameter    :

* return         void
**********************************************************/

static void NVS_save_dataPoint(void)
{
  memcpy(dataPoint_EPPROM_shadowReg,(void*)(&x_devdatapoint),sizeof(dev_data_t)); //memcpy到缓存
  for(uint8_t i=0;i<sizeof(dev_data_t);i++){
    EEPROM.write(i, dataPoint_EPPROM_shadowReg[i]);
  }
  delay(10);
  EEPROM.commit();
  DEBUG_SERIAL.println("EEPROM data write compplete");

}

/********************************************************
* Function name: system_software_reset
* Description  : 将设备数据点存入EEPROM后，重启ESP32，解决WIFI 无法连接、MQTT 连接断掉等问题
* Parameter    :

* return         void
**********************************************************/
void system_software_reset(void){
  NVS_save_dataPoint();         //save datapoint to EEPROM
  delay(1000);
  ESP.restart();                //after wifi_connect_counter times of trying, restart ESP32
}



void setup()
{
  // DEBUG_SERIAL.begin(115200);   ROS专用 57600
  rosserial_node_init();     //初始化rosserial node handle


  NVS_load_dataPoint();                 //EEPROM load函数先调用，因此在其第一行调用EEPROM.begin(size)避免遗漏！！
  setup_wifi();                          // 连接WIFI AP
  //----------ipc 对象创建--------
  g_mqtt_ctrl_msgQ = xQueueCreate(5, sizeof (ctrl_msg_queue_t));     //创建控制消息队列， mqtt callback发送至executer执行器线程

  xTaskCreateUniversal(tsk_mqtt_service, "MQTTServiceTsk",  CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 11, &mqttTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K
  xTaskCreateUniversal(tsk_executer_ctrl, "MQTTMSGCmdEXE",  CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 12, &mqttExcutorTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K
  xTaskCreateUniversal(tsk_tailling_for_strawberry, "taillingLineTsk",  CONFIG_ARDUINO_LOOP_STACK_SIZE*2, NULL, 14, &taillingTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K
  xTaskCreateUniversal(tsk_rosserial_publish, "rsoserialTsk",  CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 15, &rosserialTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K

}

void move_script_via_mq(char * p_cmd, uint32_t Kp, uint32_t Ki/*, uint32_t Kd*/){

  ctrl_msg_queue_t mqtt_ctrl_msg;                       //接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量
  mqtt_ctrl_msg.v1 = 1000;
  mqtt_ctrl_msg.v2 = 500;
  mqtt_ctrl_msg.v3 = 500;
  memcpy(mqtt_ctrl_msg.cmd, p_cmd, sizeof("CMDT"));     //命令长度固定为4byte

	if (xQueueSend(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg, 2000) == pdPASS)
  {
    DEBUG_SERIAL.println("mqtt cmd send to execute thread");
  }
}



void loop()
{

  delay(1000*1000);
// {
// "cmd":"MOVF", MOVL MOVR MOVB STOP
// "v1": 1000,   KP
// "v2": 500,    KI
// "v3": 500,    00
// "rssi":100
// }
  move_script_via_mq("MOVF", 1000, 500);             //发送消息队列到控制线程，第2个参数以后是P,I,D

  delay(10*1000);

  move_script_via_mq("MOVL", 1000, 500);             //发送消息队列到控制线程，第2个参数以后是P,I,D

  delay(20*1000);

  move_script_via_mq("STOP", 1000, 500);             //发送消息队列到控制线程，第2个参数以后是P,I,D
  for(;;) delay(10*1000);                                 //停止后阻塞到这里







}
